#include "magnetic_six.h"

uint32 left_change_time_magnetic = 0; // 定义左侧电机霍尔换向间隔时长

uint32 right_change_time_magnetic = 0; // 定义右侧电机霍尔换向间隔时长

#define ENCODER_MODULO 4681
#define SECTOR_SCALE (6.0f / ENCODER_MODULO)

#define ADVANCE_DEG_ON (DRIVER_DISABLE) // 提前换向模式
#define MAX_ADVANCE_DEG 30.0f           // 最大提前角（防止超调）
#define MAX_ADVANCE_DEG_Raw ((MAX_ADVANCE_DEG / 360.0f) * ENCODER_MODULO)
#define ADVANCE_COEFF 0.05f // 提前系数，调节精度

uint16_t get_encoder_offset_by_speed(float speed_filter, float duty)
{
    uint16_t offset = (uint16_t)(fabsf(speed_filter) * ADVANCE_COEFF);

    if (offset > MAX_ADVANCE_DEG_Raw)
        offset = (uint16_t)MAX_ADVANCE_DEG_Raw;

    return (duty >= 0) ? offset : -offset;
}

uint8 Calculate_Hall_sectors_left(void)
{
#if ADVANCE_DEG_ON == DRIVER_ENABLE
    uint16_t encoder_offset = get_encoder_offset_by_speed(motor_left.motor_speed_filter, motor_left.motor_duty); // 超前角
    uint32 a = (uint32)(motor_left.menc15a_value_now - motor_left.zero_location + encoder_offset);
#else
    uint32 a = (uint32)(motor_left.menc15a_value_now - motor_left.zero_location);
#endif
    return (uint8)((a % ENCODER_MODULO) * SECTOR_SCALE);
}
uint8 Calculate_Hall_sectors_right(void)
{
#if ADVANCE_DEG_ON == DRIVER_ENABLE
    uint16_t encoder_offset = get_encoder_offset_by_speed(motor_right.motor_speed_filter, motor_right.motor_duty); // 超前角
    uint32 a = (uint32)(motor_right.menc15a_value_now - motor_right.zero_location + encoder_offset);
#else
    uint32 a = (uint32)(motor_right.menc15a_value_now - motor_right.zero_location);
#endif
    return (uint8)((a % ENCODER_MODULO) * SECTOR_SCALE);
}

void motor_left_magnetic_output(int8 sector)
{
    uint16 output_duty_temp = 0;

    output_duty_temp = func_abs((int16)(motor_left.motor_duty * OUTPUT_DUTY_MAX));

    if (output_duty_temp == 0 && BLDC_GLIDING_MODE == DRIVER_ENABLE)
    {
        left_mos_close(); // 占空比给 0 则滑行
    }
    else
    {
        if (motor_left.motor_duty >= 0) // 设置电机占空比大于0时 执行正转偏移
        {
            // 根据计算好的输出相位调用对应的输出函数 使电机旋转至指定位置
            switch (sector)
            {
            case 4:
                left_mos_q3q2_open(output_duty_temp);
                break;
            case 2:
                left_mos_q5q4_open(output_duty_temp);
                break;
            case 3:
                left_mos_q5q2_open(output_duty_temp);
                break;
            case 0:
                left_mos_q1q6_open(output_duty_temp);
                break;
            case 5:
                left_mos_q3q6_open(output_duty_temp);
                break;
            case 1:
                left_mos_q1q4_open(output_duty_temp);
                break;
            default:
                left_mos_close();
                break;
            }
        }
        else // 设置电机反转时执行反转的偏移方向
        {
            // 根据计算好的输出相位调用对应的输出函数 使电机旋转至指定位置
            switch (sector)
            {
            case 1:
                left_mos_q3q2_open(output_duty_temp);
                break;
            case 5:
                left_mos_q5q4_open(output_duty_temp);
                break;
            case 0:
                left_mos_q5q2_open(output_duty_temp);
                break;
            case 3:
                left_mos_q1q6_open(output_duty_temp);
                break;
            case 2:
                left_mos_q3q6_open(output_duty_temp);
                break;
            case 4:
                left_mos_q1q4_open(output_duty_temp);
                break;
            default:
                left_mos_close();
                break;
            }
        }
    }
}

void motor_right_magnetic_output(int8 sector)
{
    uint16 output_duty_temp = 0;

    output_duty_temp = func_abs((int16)(motor_right.motor_duty * OUTPUT_DUTY_MAX));

    if (output_duty_temp == 0 && BLDC_GLIDING_MODE == DRIVER_ENABLE)
    {
        right_mos_close(); // 占空比给 0 则滑行
    }
    else
    {
        if (motor_right.motor_duty >= 0) // 设置电机占空比大于0时 执行正转偏移
        {
            // 根据计算好的输出相位调用对应的输出函数 使电机旋转至指定位置
            switch (sector)
            {
            case 4:
                right_mos_q3q2_open(output_duty_temp);
                break;
            case 2:
                right_mos_q5q4_open(output_duty_temp);
                break;
            case 3:
                right_mos_q5q2_open(output_duty_temp);
                break;
            case 0:
                right_mos_q1q6_open(output_duty_temp);
                break;
            case 5:
                right_mos_q3q6_open(output_duty_temp);
                break;
            case 1:
                right_mos_q1q4_open(output_duty_temp);
                break;
            default:
                right_mos_close();
                break;
            }
        }
        else // 设置电机反转时执行反转的偏移方向
        {
            // 根据计算好的输出相位调用对应的输出函数 使电机旋转至指定位置
            switch (sector)
            {
            case 1:
                right_mos_q3q2_open(output_duty_temp);
                break;
            case 5:
                right_mos_q5q4_open(output_duty_temp);
                break;
            case 0:
                right_mos_q5q2_open(output_duty_temp);
                break;
            case 3:
                right_mos_q1q6_open(output_duty_temp);
                break;
            case 2:
                right_mos_q3q6_open(output_duty_temp);
                break;
            case 4:
                right_mos_q1q4_open(output_duty_temp);
                break;
            default:
                right_mos_close();
                break;
            }
        }
    }
}

void motor_left_magnetic_callback(void)
{
    static uint8 left_speed_count = 0; // 速度拟合计次
#if MOTOR_LOCKED_PROTECT == DRIVER_ENABLE
    static uint32 left_protect_count = 0; // 输出保护计次
#endif

    motor_left.menc15a_value_now = menc15a_get_absolute_data(menc15a_1_module); // 采集左侧电机磁编码器数值

    motor_left.menc15a_value_offset = menc15a_absolute_offset_data[0]; // 获取磁编码器较上一次的偏移值

    motor_left.menc15a_offset_integral += motor_left.menc15a_value_offset; // 偏移值积分 用于速度计算

    if (++left_speed_count >= MOTOR_Speed_Number) // 每毫秒拟合一次速度
    {
        left_speed_count = 0;

        motor_left.motor_speed = (motor_left.menc15a_offset_integral * 1.8310546875f); // 速度数据拟合

        motor_left.motor_speed_filter = ((motor_left.motor_speed_filter * 19.0f + motor_left.motor_speed) * 0.05f); // 速度数据低通滤波

        motor_left.menc15a_offset_integral = 0; // 偏移积分归零

#if MOTOR_LOCKED_PROTECT == DRIVER_ENABLE
        if (motor_left.locked_value.protect_flag)
        {
            if (func_abs(motor_left.motor_speed_filter) < 10.0f && func_abs(motor_left.motor_duty) >= motor_left.locked_value.protect_duty_max) // 判断是否堵转  占空比大于10%并且无转速数据持续 500ms
            {
                left_protect_count++;

                if (left_protect_count > motor_left.locked_value.protect_check_time)
                {
                    left_protect_count = motor_left.locked_value.protect_check_time;

                    motor_left.motor_protect_state = PROTECT_MODE; // 条件成立  进入输出保护状态
                }
            }
            else
            {
                left_protect_count = 0; // 否则清空保护计次
            }
        }
#endif
    }

    motor_left.hall_value_last = motor_left.hall_value_now; // 保存上一次的霍尔数据

    motor_left.hall_value_now = Calculate_Hall_sectors_left();

    if (motor_left.motor_protect_state == PROTECT_MODE ||
        motor_left.encoder_state == ENCODER_ERROR ||
        (battery_value.protect_flag == 1 && battery_value.battery_state == BATTERY_ERROR))
    {
        motor_left_duty_set(0, 0, 0); // 输出保护状态 或者 磁编码器错误 则输出0占空比 刹车
    }
    else
    {

        if (motor_left.hall_value_now != motor_left.hall_value_last) // 判断是否换相
        {
            motor_left.run_state = MOTOR_RUN; // 成功换相则更改电机运行状态

            timer_stop(LEFT_MOTOR_COUNT_TIMER); // 停止计数器

            left_change_time_magnetic = timer_get(LEFT_MOTOR_COUNT_TIMER); // 获取换相间隔时长

            timer_clear(LEFT_MOTOR_COUNT_TIMER); // 清除定时器计数值

            timer_start(LEFT_MOTOR_COUNT_TIMER); // 启动定时器计数

            if (left_change_time_magnetic > Change_MIN_Time_Magnetic || left_change_time_magnetic == 0) // 如果换相耗时大于 最小换相耗时 或者为 0 则正常换相
            {
                motor_left_magnetic_output(motor_left.hall_value_now); // 正常换相输出
            }
        }

        if (timer_get(LEFT_MOTOR_COUNT_TIMER) > 1000 || timer_get(LEFT_MOTOR_COUNT_TIMER) == 0) // 如果换相间隔大于1s 或者 为0 则主动输出一次
        {
            motor_left_magnetic_output(motor_left.hall_value_now); // 主动换相输出
        }
    }
}

void motor_right_magnetic_callback(void)
{
    static uint8 right_speed_count = 0; // 速度拟合计次
#if MOTOR_LOCKED_PROTECT == DRIVER_ENABLE
    static uint32 right_protect_count = 0; // 输出保护计次
#endif
    motor_right.menc15a_value_now = menc15a_get_absolute_data(menc15a_2_module); // 采集左侧电机磁编码器数值

    motor_right.menc15a_value_offset = menc15a_absolute_offset_data[1]; // 获取磁编码器较上一次的偏移值

    motor_right.menc15a_offset_integral += motor_right.menc15a_value_offset; // 偏移值积分 用于速度计算

    if (++right_speed_count >= MOTOR_Speed_Number) // 每毫秒拟合一次速度
    {
        right_speed_count = 0;

        motor_right.motor_speed = (motor_right.menc15a_offset_integral * 1.8310546875f); // 速度数据拟合

        motor_right.motor_speed_filter = ((motor_right.motor_speed_filter * 19.0f + motor_right.motor_speed) * 0.05f); // 速度数据低通滤波

        motor_right.menc15a_offset_integral = 0; // 偏移积分归零

#if MOTOR_LOCKED_PROTECT == DRIVER_ENABLE
        if (motor_right.locked_value.protect_flag)
        {
            if (func_abs(motor_right.motor_speed_filter) < 10.0f && func_abs(motor_right.motor_duty) >= motor_right.locked_value.protect_duty_max) // 判断是否堵转  占空比大于 10% 并且无转速数据持续 500ms
            {
                right_protect_count++;

                if (right_protect_count > motor_right.locked_value.protect_check_time)
                {
                    right_protect_count = motor_right.locked_value.protect_check_time;

                    motor_right.motor_protect_state = PROTECT_MODE; // 条件成立  进入输出保护状态
                }
            }
            else
            {
                right_protect_count = 0; // 否则清空保护计次
            }
        }
#endif
    }

    motor_right.hall_value_last = motor_right.hall_value_now; // 保存上一次的霍尔数据

    motor_right.hall_value_now = Calculate_Hall_sectors_right();

    if (motor_right.motor_protect_state == PROTECT_MODE ||
        motor_right.encoder_state == ENCODER_ERROR ||
        (battery_value.protect_flag == 1 && battery_value.battery_state == BATTERY_ERROR))
    {
        motor_right_duty_set(0, 0, 0); // 保护状态输出0占空比 刹车
    }
    else
    {
        if (motor_right.hall_value_now != motor_right.hall_value_last) // 判断是否换相
        {
            motor_right.run_state = MOTOR_RUN; // 成功换相则更改电机运行状态

            timer_stop(RIGHT_MOTOR_COUNT_TIMER); // 停止计数器

            right_change_time_magnetic = timer_get(RIGHT_MOTOR_COUNT_TIMER); // 获取换相间隔时长

            timer_clear(RIGHT_MOTOR_COUNT_TIMER); // 清除定时器计数值

            timer_start(RIGHT_MOTOR_COUNT_TIMER); // 启动定时器计数

            if (right_change_time_magnetic > Change_MIN_Time_Magnetic || right_change_time_magnetic == 0) // 如果换相耗时大于 250us 或者为 0 则正常换相
            {
                motor_right_magnetic_output(motor_right.hall_value_now); // 正常换相输出
            }
        }

        if (timer_get(RIGHT_MOTOR_COUNT_TIMER) > 1000 || timer_get(RIGHT_MOTOR_COUNT_TIMER) == 0) // 如果换相间隔大于1ms 或者 为0 则主动输出一次
        {
            motor_right_magnetic_output(motor_right.hall_value_now); // 主动换相输出
        }
    }
}

void magnetic_six_init(void)
{
    motor_left.forward_traction_angle = FOC_FORWARD_TRACTION_ANGLE;

    motor_left.reversal_traction_angle = FOC_REVERSAL_TRACTION_ANGLE;

    motor_right.forward_traction_angle = FOC_FORWARD_TRACTION_ANGLE;

    motor_right.reversal_traction_angle = FOC_REVERSAL_TRACTION_ANGLE;

    menc15a_init(); // 磁编码器初始化

    fast_foc_init(&motor_left_foc_driver, ENCODER_PRECISION, FOC_PWM_PRIOD_LOAD, motor_left.pole_pairs, motor_left.zero_location, motor_left.rotation_direction); // 左侧电机 FAST_FOC 功能初始化

    fast_foc_init(&motor_right_foc_driver, ENCODER_PRECISION, FOC_PWM_PRIOD_LOAD, motor_right.pole_pairs, motor_right.zero_location, motor_right.rotation_direction); // 右侧电机 FAST_FOC 功能初始化

    motor_left_output_init(PWM_PRIOD_LOAD, 0); // 左侧电机三相 PWM 输出初始化

    motor_right_output_init(PWM_PRIOD_LOAD, 0); // 右侧电机三相 PWM 输出初始化

    pit_us_init(LEFT_MOTOR_PIT_TIMER, MOTOR_PIT_TIMER); // 初始化 左侧电机 轮询周期中断

    timer_init(LEFT_MOTOR_COUNT_TIMER, TIMER_US); // 初始化 左侧电机计数定时器

    pit_us_init(RIGHT_MOTOR_PIT_TIMER, MOTOR_PIT_TIMER); // 初始化 右侧电机 轮询周期中断

    timer_init(RIGHT_MOTOR_COUNT_TIMER, TIMER_US); // 初始化 右侧电机计数定时器
}
